﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Text;
using System.Windows.Forms;
using LocalizationTest.Tools;
using System.Drawing.Imaging;
using System.IO;
using System.Reflection;

namespace LocalizationTest
{
  public partial class MainForm : Form
  {
    //SCIENCE
    Experiment experiment;
    List<SelfLocator> selfLocatorList = new List<SelfLocator>();
    Dictionary<string, SelfLocatorProperties> selfLocatorPropertiesDictionary = new Dictionary<string, SelfLocatorProperties>();
    SelfLocalizationResultHistory selfLocalizationResultHistory = new SelfLocalizationResultHistory();
    ContinuousAction2DCreator actionCreator = new ContinuousAction2DCreator();

    //AVI AND IMAGE EXPORT AND LOGFILE SUPPORT
    public int firstIndexOfCurrentFrame = 0;
    LogPlayer logPlayer;

    //VISUALIZATION
    bool showSelections = true;
    bool showTextOutput = false;
    double zoom = 1.0;

    public MainForm()
    {
      InitializeComponent();

      //INITIALIZATIONS
      ConfigurationTool.InitConfiguration(DrawingTools.GetPath() + "configuration.xml");

      InitSelections();
      CreateSelfLocators();
      FillComboBoxWithListOfExperiments();
      Globals.Init(experiment);
      experimentConfigurator.Init(experiment);
      experimentConfigurator.ConfigurationChanged += new EventHandler(experimentConfigurator_ConfigurationChanged);

      propertyGridLocalization.SelectedObject = selfLocatorPropertiesDictionary["default"];
      
      logPlayer = new LogPlayer(progressBar);
    }

    #region initialization
    private void InitSelections()
    {
      nameSpaceTreeViewDrawingSelection.TreeView.AfterCheck += new TreeViewEventHandler(treeViewDrawingSelection_AfterCheck);
      DebugDrawingTool.SetNameSpaceTreeView(nameSpaceTreeViewDrawingSelection);
      DebugDrawingTool.SetDrawingPanelContainer(drawingPanelContainer);
      nameSpaceTreeViewDrawingSelection.Separator = ':';
      nameSpaceTreeViewDrawingSelection.UpdateTree();

      nameSpaceTreeViewDebugKeys.TreeView.AfterCheck += new TreeViewEventHandler(treeViewDebugKeys_AfterCheck);
      DebugKeyTool.SetNameSpaceTreeView(nameSpaceTreeViewDebugKeys);
      nameSpaceTreeViewDebugKeys.Separator = ':';
      nameSpaceTreeViewDebugKeys.UpdateTree();
    }
    private void FillComboBoxWithListOfExperiments()
    {
      string lastExperiment = ConfigurationTool.Configuration.ReadConfigurationEntry("experiment", "Experiment2DDistanceSensor");
      Assembly assembly = Assembly.GetEntryAssembly();
      foreach (Module module in assembly.GetModules())
      {
        foreach (Type type in module.GetTypes())
        {
          if (type.IsSubclassOf(typeof(Experiment)))
          {
            comboBoxExperiment.Items.Add(type.Name);
            if (type.Name == lastExperiment)
              experiment = (Experiment)System.Activator.CreateInstance(type);
          }
        }
      }
      comboBoxExperiment.Sorted = true;
      comboBoxExperiment.Text = experiment.GetType().Name;
      comboBoxExperiment.SelectedValueChanged += new EventHandler(comboBoxExperiment_SelectedValueChanged);
    }
    private void CreateSelfLocators()
    {
      Assembly assembly = Assembly.GetEntryAssembly();
      foreach (Module module in assembly.GetModules())
      {
        foreach (Type type in module.GetTypes())
        {
          if (type.IsSubclassOf(typeof(SelfLocator)))
          {
            selfLocatorList.Add((SelfLocator)System.Activator.CreateInstance(type) );
          }
        }
      }
      selfLocatorPropertiesDictionary.Add("default", new SelfLocatorProperties());
      selfLocatorPropertiesDictionary["default"].MotionNoise = NoiseType.gaussianNoSystematic;
      selfLocatorPropertiesDictionary["default"].SensorNoise = NoiseType.gaussianNoSystematic;
      selfLocatorPropertiesDictionary["default"].SelectionStrategy = LandmarkSelectionStrategy.lastAndFirstOfEachInLastN;
      selfLocatorPropertiesDictionary["default"]._MaxNumberOfObservations = 5;
      selfLocatorPropertiesDictionary["default"]._MinSizeForBlocks = 3;

      /*
      selfLocatorPropertiesDictionary.Add("default2", new SelfLocatorProperties());
      selfLocatorPropertiesDictionary["default2"].SelectionStrategy = LandmarkSelectionStrategy.lastAndFirstOfEachInLastN;
      selfLocatorPropertiesDictionary["default2"]._MaxNumberOfObservations = 7;
      selfLocatorPropertiesDictionary["default2"]._MinSizeForBlocks = 3;
      */

      /*
      selfLocatorPropertiesDictionary.Add("last3OfEach", new SelfLocatorProperties());
      selfLocatorPropertiesDictionary["last3OfEach"].SelectionStrategy = LandmarkSelectionStrategy.lastNOfEach;
      selfLocatorPropertiesDictionary["last3OfEach"].ParamLastNOfEach = 3;
      */
/*
      selfLocatorPropertiesDictionary.Add("all", new SelfLocatorProperties());
      selfLocatorPropertiesDictionary["all"].SelectionStrategy = LandmarkSelectionStrategy.all;

      selfLocatorPropertiesDictionary.Add("last10", new SelfLocatorProperties());
      selfLocatorPropertiesDictionary["last10"].SelectionStrategy = LandmarkSelectionStrategy.lastN;
      selfLocatorPropertiesDictionary["last10"].ParamLastN = 10;

      selfLocatorPropertiesDictionary.Add("last20", new SelfLocatorProperties());
      selfLocatorPropertiesDictionary["last20"].SelectionStrategy = LandmarkSelectionStrategy.lastN;
      selfLocatorPropertiesDictionary["last20"].ParamLastN = 20;
 */

      /*
      selfLocatorPropertiesDictionary.Add("noError", new SelfLocatorProperties());
      selfLocatorPropertiesDictionary["noError"].SelectionStrategy = LandmarkSelectionStrategy.lastNOfEach;
      selfLocatorPropertiesDictionary["noError"].ParamLastNOfEach = 1;
      selfLocatorPropertiesDictionary["noError"].MotionNoise = NoiseType.noNoise;
      selfLocatorPropertiesDictionary["noError"].SensorNoise = NoiseType.noNoise;
       */
 
/*      
      selfLocatorPropertiesDictionary.Add("lastOfEach", new SelfLocatorProperties());
      selfLocatorPropertiesDictionary["lastOfEach"].SelectionStrategy = LandmarkSelectionStrategy.lastNOfEach;
      selfLocatorPropertiesDictionary["lastOfEach"].ParamLastNOfEach = 1;

      selfLocatorPropertiesDictionary.Add("last5OfEach", new SelfLocatorProperties());
      selfLocatorPropertiesDictionary["last5OfEach"].SelectionStrategy = LandmarkSelectionStrategy.lastNOfEach;
      selfLocatorPropertiesDictionary["last5OfEach"].ParamLastNOfEach = 5;
 */ 
    }
    #endregion
    #region key handling
    enum KeyhandleResult { none, moveAndMeasure, selectionChanged };
    
    private void MainForm_KeyDown(object sender, KeyEventArgs e)
    {
      //KeyPressEventArgs.Handled = false
      HandleKeyMain(e);
    }
    private void drawingPanelContainer_KeyDown(object sender, KeyEventArgs e)
    {
      HandleKeyMain(e);
    }

    private void HandleKeyMain(KeyEventArgs e)
    {
      labelCalculationTimes.Visible = DebugKeyTool.IsActive("options view:time measurements");
      labelHelp.Visible = DebugKeyTool.IsActive("options view:help");
      if (!DebugKeyTool.IsActive("options view:keep drawings"))
      {
        DebugDrawingTool.ResetDrawings();
      }
      if (DebugKeyTool.IsActive("small motion raster")) Globals.motionRasterWidth = 4; 

      bool motionSelected = false;
      bool measurementSelected = false;

      ActionDescription selectedAction = null;

      KeyhandleResult keyhandleResult = KeyhandleResult.none;
      if (showSelections) keyhandleResult = HandleKeysForDrawingSelection(e, keyhandleResult);
      if (keyhandleResult == KeyhandleResult.none) keyhandleResult = HandleKeysForDebugKeySelection(e, keyhandleResult);
      if (keyhandleResult == KeyhandleResult.none) keyhandleResult = HandleControlKeys(e, keyhandleResult);
      if (experiment.typeOfActionDescription == typeof(DiscreteAction1D))
      {
        if (keyhandleResult == KeyhandleResult.none) keyhandleResult = HandleMoveAndMeasureKeysDiscrete1D(e, ref motionSelected, ref measurementSelected, ref selectedAction, keyhandleResult);
      }
      else if (experiment.typeOfActionDescription == typeof(DiscreteAction2D))
      {
        if (keyhandleResult == KeyhandleResult.none) keyhandleResult = HandleMoveAndMeasureKeysDiscrete2D(e, ref motionSelected, ref measurementSelected, ref selectedAction, keyhandleResult);
      }
      else if (experiment.typeOfActionDescription == typeof(ContinuousAction2D))
      {
        if (keyhandleResult == KeyhandleResult.none) keyhandleResult = HandleMoveAndMeasureKeysContinuousTarget(e, ref motionSelected, ref measurementSelected, ref selectedAction, keyhandleResult);
      }
      else if (experiment.typeOfActionDescription == typeof(ContinuousActionPose2D))
      {
        if (keyhandleResult == KeyhandleResult.none) keyhandleResult = HandleMoveAndMeasureKeysContinuousPose2DTarget(e, ref motionSelected, ref measurementSelected, ref selectedAction, keyhandleResult);
      }

      //SIMULATION AND BUFFERING
      DebugTextTool.Output("simulation" + firstIndexOfCurrentFrame);
      if (motionSelected || measurementSelected)
        firstIndexOfCurrentFrame = experiment.actionAndObservationHistory.history.Count;
      if (motionSelected)
      {
        experiment.simulatedWorldState = selectedAction.Execute(experiment.motionModel, experiment.simulatedWorldState);
        experiment.actionAndObservationHistory.Add(experiment.simulatedWorldState);
        experiment.actionAndObservationHistory.Add(selectedAction);
      }
      if (DebugKeyTool.IsActive("sparse measurements") && experiment.actionAndObservationHistory.history.Count % 10 > 1)
      {
        measurementSelected = false;
      }
      if (measurementSelected)
      {
        foreach (Sensor sensor in experiment.sensors)
        {
          sensor.SimulateMeasurement(experiment.simulatedWorldState, experiment.map, experiment.actionAndObservationHistory);
//          Measurement performedMeasurement = sensor.PredictForSimulation(experiment.simulatedWorldState, experiment.map);
//          experiment.actionAndObservationHistory.Add(performedMeasurement);
//          performedMeasurement.Draw(experiment.simulatedWorldState, 0);
        }
      }

      //DRAW MAP
      if (!DebugKeyTool.IsActive("options view:map - draw last")) experiment.map.Draw();

      Localization();

      //DRAW MAP
      if (DebugKeyTool.IsActive("options view:map - draw last")) experiment.map.Draw();

      DebugDrawingTool.CreateCircle("world:action creator:target", actionCreator.target, 5, 7, Color.Red);

      //DRAW WORLDSTATE
      experiment.simulatedWorldState.DrawHighlighted("world:_world state", Color.Blue);

      //DRAW ALL NEW MEASUREMENTS
      int index = firstIndexOfCurrentFrame;
      while (index >= 0 && index < experiment.actionAndObservationHistory.history.Count)
      {
        if (experiment.actionAndObservationHistory.history[index] is Measurement)
        {
          (experiment.actionAndObservationHistory.history[index] as Measurement).Draw(experiment.simulatedWorldState, 0, "world:_measurement");
        }
        index++;
      }

      //DRAW HISTORY
      experiment.actionAndObservationHistory.Draw(experiment.simulatedWorldState);

      InvalidatePanels();

      DebugDrawingTool.SaveBitmaps();
      DebugDrawingTool.SaveMetafiles();

      //RecordMatafiles(motionSelected, measurementSelected);
      TimeTool.Show(labelCalculationTimes);

      if (showTextOutput)
      {
        richTextBox.Focus();
        richTextBox.Text = DebugTextTool.GetText();
        if (richTextBox.Text.Length > 1) richTextBox.Select(richTextBox.Text.Length - 2, 1);
      }
    }
    public void InvalidatePanels()
    {
      this.Invalidate();
      drawingPanelContainer.Invalidate();
      foreach(Control control in drawingPanelContainer.Controls)
      {
        control.Invalidate();
      }
    }

    private KeyhandleResult HandleControlKeys(KeyEventArgs e, KeyhandleResult keyhandleResult)
    {
      switch (e.KeyCode)
      {
        case Keys.L:
          {
            experiment.actionAndObservationHistory.Save();
            break;
          }
        case Keys.O:
          {
            logPlayer.Open(experiment);
            break;
          }
        case Keys.N:
          {
            firstIndexOfCurrentFrame = experiment.actionAndObservationHistory.history.Count;
            logPlayer.Next(experiment);
            break;
          }
        case Keys.B:
          {
            logPlayer.Play(this, experiment);
            break;
          }
        case Keys.Home:
          {
            zoom = 1.0;
            drawingPanelContainer.ResetZoom();
            InvalidatePanels();
            break;
          }
        case Keys.PageUp:
          {
            zoom *= 1.1;
            drawingPanelContainer.ZoomInCurrentPanel();
            InvalidatePanels();
            break;
          }
        case Keys.PageDown:
          {
            zoom /= 1.1;
            drawingPanelContainer.ZoomOutCurrentPanel();
            InvalidatePanels();
            break;
          }
        case Keys.P:
          {
            showSelections = !showSelections;
            if (showSelections)
            {
              panelSelections.Visible = true;
            }
            else
            {
              panelSelections.Visible = false;
              drawingPanelContainer.Focus();
            }
            break;
          }
        case Keys.T:
          {
            showTextOutput = !showTextOutput;
            if (showTextOutput)
            {
              richTextBox.Visible = true;
              richTextBox.Focus();
              richTextBox.Text = DebugTextTool.GetText();
              if (richTextBox.Text.Length > 1) richTextBox.Select(richTextBox.Text.Length - 2, 1);
            }
            else
            {
              richTextBox.Visible = false;
            }
            break;
          }
        case Keys.C:
          {
            DebugDrawingTool.ResetDrawingIDs();
            break;
          }

        default:
          keyhandleResult = KeyhandleResult.none;
          break;
      }
      return keyhandleResult;
    }
    private KeyhandleResult HandleMoveAndMeasureKeysDiscrete1D(KeyEventArgs e, ref bool motionSelected, ref bool measurementSelected, ref ActionDescription selectedAction, KeyhandleResult keyhandleResult)
    {
      selectedAction = new DiscreteAction1D(DiscreteAction1D.TypeOfAction.none);
      switch (e.KeyCode)
      {
        case Keys.R:
          {
            experiment.actionAndObservationHistory.Delete();
            selfLocalizationResultHistory.Delete();
            DebugTextTool.Reset();
            //DrawingTool.Reset();
            InvalidatePanels();
            break;
          }
        case Keys.K:
          {
            experiment.simulatedWorldState.Kidnap();
            InvalidatePanels();
            break;
          }
        case Keys.Left:
          {
            (selectedAction as DiscreteAction1D).typeOfAction = DiscreteAction1D.TypeOfAction.moveLeft;
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Right:
          {
            (selectedAction as DiscreteAction1D).typeOfAction = DiscreteAction1D.TypeOfAction.moveRight;
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Space:
          {
            (selectedAction as DiscreteAction1D).typeOfAction = DiscreteAction1D.TypeOfAction.doNothing;
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.A:
          {
            (selectedAction as DiscreteAction1D).typeOfAction = DiscreteAction1D.TypeOfAction.moveLeft;
            motionSelected = true;
            break;
          }
        case Keys.D:
          {
            (selectedAction as DiscreteAction1D).typeOfAction = DiscreteAction1D.TypeOfAction.moveRight;
            motionSelected = true;
            break;
          }
        case Keys.S:
          {
            (selectedAction as DiscreteAction1D).typeOfAction = DiscreteAction1D.TypeOfAction.doNothing;
            motionSelected = true;
            break;
          }
        case Keys.M:
          {
            measurementSelected = true;
            break;
          }

        default:
          keyhandleResult = KeyhandleResult.none;
          break;
      }
      return keyhandleResult;
    }
    private KeyhandleResult HandleMoveAndMeasureKeysDiscrete2D(KeyEventArgs e, ref bool motionSelected, ref bool measurementSelected, ref ActionDescription selectedAction, KeyhandleResult keyhandleResult)
    {
      selectedAction = new DiscreteAction2D(DiscreteAction2D.TypeOfAction.none);
      switch (e.KeyCode)
      {
        case Keys.R:
          {
            experiment.actionAndObservationHistory.Delete();
            selfLocalizationResultHistory.Delete();
            DebugTextTool.Reset();
            //DrawingTool.Reset();
            InvalidatePanels();
            break;
          }
        case Keys.K:
          {
            experiment.simulatedWorldState.Kidnap();
            InvalidatePanels();
            break;
          }
        case Keys.Left:
          {
            (selectedAction as DiscreteAction2D).typeOfAction = DiscreteAction2D.TypeOfAction.moveLeft;
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Right:
          {
            (selectedAction as DiscreteAction2D).typeOfAction = DiscreteAction2D.TypeOfAction.moveRight;
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Up:
          {
            (selectedAction as DiscreteAction2D).typeOfAction = DiscreteAction2D.TypeOfAction.moveUp;
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Down:
          {
            (selectedAction as DiscreteAction2D).typeOfAction = DiscreteAction2D.TypeOfAction.moveDown;
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Space:
          {
            (selectedAction as DiscreteAction2D).typeOfAction = DiscreteAction2D.TypeOfAction.doNothing;
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.A:
          {
            (selectedAction as DiscreteAction2D).typeOfAction = DiscreteAction2D.TypeOfAction.moveLeft;
            motionSelected = true;
            break;
          }
        case Keys.D:
          {
            (selectedAction as DiscreteAction2D).typeOfAction = DiscreteAction2D.TypeOfAction.moveRight;
            motionSelected = true;
            break;
          }
        case Keys.W:
          {
            (selectedAction as DiscreteAction2D).typeOfAction = DiscreteAction2D.TypeOfAction.moveUp;
            motionSelected = true;
            break;
          }
        case Keys.X:
          {
            (selectedAction as DiscreteAction2D).typeOfAction = DiscreteAction2D.TypeOfAction.moveDown;
            motionSelected = true;
            break;
          }
        case Keys.S:
          {
            (selectedAction as DiscreteAction2D).typeOfAction = DiscreteAction2D.TypeOfAction.doNothing;
            motionSelected = true;
            break;
          }
        case Keys.M:
          {
            measurementSelected = true;
            break;
          }

        default:
          keyhandleResult = KeyhandleResult.none;
          break;
      }
      return keyhandleResult;
    }
    private KeyhandleResult HandleMoveAndMeasureKeysContinuous(KeyEventArgs e, ref bool motionSelected, ref bool measurementSelected, ref ContinuousAction2D selectedAction, KeyhandleResult keyhandleResult)
    {
      switch (e.KeyCode)
      {
        case Keys.R:
          {
            experiment.actionAndObservationHistory.Delete();
            selfLocalizationResultHistory.Delete();
            if (experiment.simulatedWorldState is WorldState2D)
            {
              actionCreator.Reset(new Pose2D(
                (experiment.simulatedWorldState as WorldState2D).Vector.x,
                (experiment.simulatedWorldState as WorldState2D).Vector.y, 0));
            }
            //DrawingTool.Reset();
            DebugTextTool.Reset();
            InvalidatePanels();
            break;
          }
        case Keys.K:
          {
            experiment.simulatedWorldState.Kidnap();
            InvalidatePanels();
            break;
          }
        case Keys.Left:
          {
            actionCreator.currentRotationSpeed += 0.01;
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Right:
          {
            actionCreator.currentRotationSpeed -= 0.01;
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Up:
          {
            actionCreator.currentSpeed += 1;
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Down:
          {
            actionCreator.currentSpeed -= 1;
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Space:
          {
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.A:
          {
            actionCreator.currentRotationSpeed += 0.01;
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            break;
          }
        case Keys.D:
          {
            actionCreator.currentRotationSpeed -= 0.01;
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            break;
          }
        case Keys.W:
          {
            actionCreator.currentSpeed += 1;
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            break;
          }
        case Keys.X:
          {
            actionCreator.currentSpeed -= 1;
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            break;
          }
        case Keys.S:
          {
            actionCreator.currentSpeed = 0;
            actionCreator.currentRotationSpeed = 0;
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            break;
          }
        case Keys.M:
          {
            measurementSelected = true;
            break;
          }

        default:
          keyhandleResult = KeyhandleResult.none;
          break;
      }
      return keyhandleResult;
    }
    private KeyhandleResult HandleMoveAndMeasureKeysContinuousTarget(KeyEventArgs e, ref bool motionSelected, ref bool measurementSelected, ref ActionDescription selectedAction, KeyhandleResult keyhandleResult)
    {
      selectedAction = new ContinuousAction2D(new Vector2());
      switch (e.KeyCode)
      {
        case Keys.R:
          {
            experiment.actionAndObservationHistory.Delete();
            selfLocalizationResultHistory.Delete();
            if (experiment.simulatedWorldState is WorldState2D)
            {
              actionCreator.Reset(new Pose2D(
                (experiment.simulatedWorldState as WorldState2D).Vector.x,
                (experiment.simulatedWorldState as WorldState2D).Vector.y, 0));
            }
            DebugTextTool.Reset();
            //DrawingTool.Reset();
            InvalidatePanels();
            break;
          }
        case Keys.K:
          {
            experiment.simulatedWorldState.Kidnap();
            InvalidatePanels();
            break;
          }
        case Keys.Left:
          {
            actionCreator.target.x -= 25;
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Right:
          {
            actionCreator.target.x += 25;
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Up:
          {
            actionCreator.target.y += 25;
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Down:
          {
            actionCreator.target.y -= 25;
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Space:
          {
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.A:
          {
            actionCreator.target.x -= 10;
            motionSelected = true;
            break;
          }
        case Keys.D:
          {
            actionCreator.target.x += 10;
            motionSelected = true;
            break;
          }
        case Keys.W:
          {
            actionCreator.target.y += 10;
            motionSelected = true;
            break;
          }
        case Keys.X:
          {
            actionCreator.target.y -= 10;
            motionSelected = true;
            break;
          }
        case Keys.S:
          {
            (selectedAction as ContinuousAction2D).action = actionCreator.GetPositionDifference((experiment.simulatedWorldState as WorldState2D));
            motionSelected = true;
            break;
          }
        case Keys.M:
          {
            measurementSelected = true;
            break;
          }

        default:
          keyhandleResult = KeyhandleResult.none;
          break;
      }
      DebugDrawingTool.CreateCircle("world:action creator:target", actionCreator.target, 15, 2, Color.Red);

      return keyhandleResult;
    }
    private KeyhandleResult HandleMoveAndMeasureKeysContinuousPose2DTarget(KeyEventArgs e, ref bool motionSelected, ref bool measurementSelected, ref ActionDescription selectedAction, KeyhandleResult keyhandleResult)
    {
      selectedAction = new ContinuousActionPose2D(new Pose2D());
      switch (e.KeyCode)
      {
        case Keys.R:
          {
            experiment.actionAndObservationHistory.Delete();
            selfLocalizationResultHistory.Delete();
            if (experiment.simulatedWorldState is WorldStatePose2D)
            {
              actionCreator.Reset((experiment.simulatedWorldState as WorldStatePose2D).Pose);
            }
            //DrawingTool.Reset();
            InvalidatePanels();
            DebugTextTool.Reset();
            break;
          }
        case Keys.K:
          {
            experiment.simulatedWorldState.Kidnap();
            InvalidatePanels();
            break;
          }
        case Keys.Left:
          {
            actionCreator.target.x -= 25;
            (selectedAction as ContinuousActionPose2D).action = actionCreator.GetPoseDifference((experiment.simulatedWorldState as WorldStatePose2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Right:
          {
            actionCreator.target.x += 25;
            (selectedAction as ContinuousActionPose2D).action = actionCreator.GetPoseDifference((experiment.simulatedWorldState as WorldStatePose2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Up:
          {
            actionCreator.target.y += 25;
            (selectedAction as ContinuousActionPose2D).action = actionCreator.GetPoseDifference((experiment.simulatedWorldState as WorldStatePose2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Down:
          {
            actionCreator.target.y -= 25;
            (selectedAction as ContinuousActionPose2D).action = actionCreator.GetPoseDifference((experiment.simulatedWorldState as WorldStatePose2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.Space:
          {
            (selectedAction as ContinuousActionPose2D).action = actionCreator.GetPoseDifference((experiment.simulatedWorldState as WorldStatePose2D));
            motionSelected = true;
            measurementSelected = true;
            break;
          }
        case Keys.A:
          {
            actionCreator.target.x -= 10;
            motionSelected = true;
            break;
          }
        case Keys.D:
          {
            actionCreator.target.x += 10;
            motionSelected = true;
            break;
          }
        case Keys.W:
          {
            actionCreator.target.y += 10;
            motionSelected = true;
            break;
          }
        case Keys.X:
          {
            actionCreator.target.y -= 10;
            motionSelected = true;
            break;
          }
        case Keys.S:
          {
            (selectedAction as ContinuousActionPose2D).action = actionCreator.GetPoseDifference((experiment.simulatedWorldState as WorldStatePose2D));
            motionSelected = true;
            break;
          }
        case Keys.M:
          {
            measurementSelected = true;
            break;
          }

        default:
          keyhandleResult = KeyhandleResult.none;
          break;
      }
      return keyhandleResult;
    }
    private KeyhandleResult HandleKeysForDebugKeySelection(KeyEventArgs e, KeyhandleResult keyhandleResult)
    {
      switch (e.KeyCode)
      {
        case Keys.F1:
          {
            DebugKeyTool.ToggleKey("options view:help");
            break;
          }
        default:
          keyhandleResult = KeyhandleResult.none;
          break;
      }
      return keyhandleResult;
    }
    private KeyhandleResult HandleKeysForDrawingSelection(KeyEventArgs e, KeyhandleResult keyhandleResult)
    {
      if (e.KeyCode == Keys.P || e.KeyCode == Keys.Escape)
      {
        showSelections = !showSelections;
        if (showSelections)
        {
          panelSelections.Visible = true;
        }
        else
        {
          panelSelections.Visible = false;
        }
        return KeyhandleResult.selectionChanged;
      }
      return KeyhandleResult.none;
    }
    #endregion
    #region selection changed
    void treeViewDrawingSelection_AfterCheck(object sender, TreeViewEventArgs e)
    {
      drawingPanelContainer.Invalidate();
    }
    void treeViewDebugKeys_AfterCheck(object sender, TreeViewEventArgs e)
    {
      drawingPanelContainer.Invalidate();
    }
    void comboBoxExperiment_SelectedValueChanged(object sender, EventArgs e)
    {
      Type type = Type.GetType(this.GetType().Namespace + "." + comboBoxExperiment.Text);
      experiment = (Experiment)System.Activator.CreateInstance(type);
      experimentConfigurator.Init(experiment);
      InitAfterExperimentChanged();
      profileManager1.ReOpenProfile();
      ConfigurationTool.Configuration.WriteConfigurationEntry("experiment", comboBoxExperiment.Text);
    }
    private void InitAfterExperimentChanged()
    {
      nameSpaceTreeViewDrawingSelection.Focus();
      experiment.actionAndObservationHistory.Delete();
      selfLocalizationResultHistory.Delete();
      Globals.Init(experiment);
      DebugKeyTool.Reset();
      DebugDrawingTool.ResetDrawingIDs();
      InvalidatePanels();
    }
    #endregion

    public void Localization()
    {
      //LOCALIZATION
      //int index = 0;
      foreach (SelfLocator selfLocator in selfLocatorList)
      {
        string selfLocatorName = selfLocator.GetType().Name;
        if (DebugKeyTool.IsActive("SelfLocator:" + selfLocatorName))
        {
          if (selfLocatorName == "ground_truth" || selfLocatorName == "odometry")
            RunSelfLocator(selfLocator, "");
          else
          {
            foreach (string propertySet in selfLocatorPropertiesDictionary.Keys)
            {
              RunSelfLocator(selfLocator, propertySet);
            }
          }
//          bestState.DrawHighlighted("world:result:position:" + selfLocator.GetType().Name, SelfLocalizationResultHistory.GetSelfLocatorColor(selfLocator.GetType().Name));
          //          Vector2 offset = new Vector2(-200, 0);
          //          int diff = 40;
          //          DebugDrawingTool.CreateLine("legend", new Vector2(10, diff * index) + offset, new Vector2(40, diff * index) + offset, 4, SelfLocalizationResultHistory.GetSelfLocatorColor(selfLocator));
          //          DebugDrawingTool.CreateText("legend", selfLocator.GetType().Name.Replace("_"," "), new Vector2(40, diff * index) + offset, 30, Color.Black);
          //index++;
        }
      }

      DebugDrawingTool.CreateText("world:text:odo err",
        "odometry error", new Vector2(0, 0), 40, Color.Blue);

      DebugDrawingTool.CreateText("world:text:memory all",
        "visualization of memory", new Vector2(0, 0), 40, Color.Blue);

      selfLocalizationResultHistory.FinishFrame();
    }

    private void RunSelfLocator(SelfLocator selfLocator, string propertySet)
    {
      string selfLocatorName = selfLocator.GetType().Name;
      TimeTool.Start("SelfLocator:" + selfLocatorName + " " + propertySet);
      WorldState bestState;

      if (propertySet == "") Globals.selfLocatorProperties = selfLocatorPropertiesDictionary["default"];
      else Globals.selfLocatorProperties = selfLocatorPropertiesDictionary[propertySet];
      
      bestState = selfLocator.Execute(experiment);

      if (bestState != null)
      {
        if (propertySet == "") selfLocalizationResultHistory.Add(selfLocatorName, bestState);
        else selfLocalizationResultHistory.Add(selfLocatorName + " " + propertySet, bestState);
      }

      TimeTool.Stop("SelfLocator:" + selfLocatorName + " " + propertySet);
    }

    void experimentConfigurator_ConfigurationChanged(object sender, EventArgs e)
    {
      InitAfterExperimentChanged();
    }
    private void MainForm_Resize(object sender, EventArgs e)
    {
      this.Invalidate();
    }
    private void MainForm_KeyPress(object sender, KeyPressEventArgs e)
    {

    }
  }
}

/* Metafile ...

    #region avi and image methods
    private void RecordMatafiles(bool motionSelected, bool measurementSelected)
    {
      if (DebugKeyTool.IsActive("export:save everything to emf") )
      {
        emfExportCounter++;
        string fileNameBase =
          DrawingTools.GetVideoOutputPath() +
          DateTime.Now.Date.Year + "-" +
          DateTime.Now.Date.Month.ToString("00") + "-" +
          DateTime.Now.Date.Day +
          "_" +
          sessionCounter.ToString("00") + "_" +
          emfExportCounter.ToString("000") + "_";

        CreateMetaFileForWorld(fileNameBase);
        CreateMetaFileForMap(fileNameBase);
        CreateMetaFileForSimulatedMeasurement(fileNameBase);
        CreateMetaFileForSimulatedWorldState(fileNameBase);
        CreateMetaFileForHistory(fileNameBase);
        DebugDrawingTool.CreateMetafiles(fileNameBase);
      }
    }
    private void CreateMetaFileForHistory(string fileNameBase)
    {
      Metafile mf;
      Graphics graphics;
      DrawingTools.CreateMetaFileGraphics(fileNameBase + "history.emf", out mf, out graphics);
//      graphics.ScaleTransform(1, -1);
      experiment.actionAndObservationHistory.Draw(graphics, experiment.simulatedWorldState);
      DrawingTools.FinishMetaFileGraphics(mf, graphics);
    }
    private void CreateMetaFileForWorld(string fileNameBase)
    {
      Metafile mf;
      Graphics graphics;
      DrawingTools.CreateMetaFileGraphics(fileNameBase + "world.emf", out mf, out graphics);
      graphics.ScaleTransform(1, -1);
      PaintWorld(graphics, 250, 250);
      DrawingTools.FinishMetaFileGraphics(mf, graphics);
    }
    private void CreateMetaFileForMap(string fileNameBase)
    {
      Metafile mf;
      Graphics graphics;
      DrawingTools.CreateMetaFileGraphics(fileNameBase + "map.emf", out mf, out graphics);
      PaintMapOnly(graphics);
      DrawingTools.FinishMetaFileGraphics(mf, graphics);
    }
    private void CreateMetaFileForSimulatedMeasurement(string fileNameBase)
    {
      Metafile mf;
      Graphics graphics;
      DrawingTools.CreateMetaFileGraphics(fileNameBase + "simulated-measurement.emf", out mf, out graphics);
      PaintSimulatedMeasurementOnly(graphics);
      DrawingTools.FinishMetaFileGraphics(mf, graphics);
    }
    private void CreateMetaFileForSimulatedWorldState(string fileNameBase)
    {
      Metafile mf;
      Graphics graphics;
      DrawingTools.CreateMetaFileGraphics(fileNameBase + "simulated-world-state.emf", out mf, out graphics);
      PaintSimulatedWorldStateOnly(graphics);
      DrawingTools.FinishMetaFileGraphics(mf, graphics);
    }
    #endregion
*/